home *** CD-ROM | disk | FTP | other *** search
/ Cream of the Crop 26 / Cream of the Crop 26.iso / os2 / plnk081.zip / pilot-link.0.8.1 / libsock / syspkt.c < prev    next >
C/C++ Source or Header  |  1997-07-03  |  13KB  |  691 lines
  1. /* syspkt.c:  Pilot SysPkt manager
  2.  *
  3.  * (c) 1996, Kenneth Albanowski.
  4.  * Derived from padp.c.
  5.  *
  6.  * This is free software, licensed under the GNU Public License V2.
  7.  * See the file COPYING for details.
  8.  */
  9.  
  10. #include <stdio.h>
  11. #include <stdarg.h>
  12.  
  13. #include "pi-source.h"
  14. #include "pi-socket.h"
  15. #include "pi-syspkt.h"
  16. #include "pi-slp.h"
  17. #include "pi-serial.h"
  18.  
  19. int sys_RPCerror;
  20.  
  21. int syspkt_tx(struct pi_socket *ps, void *m, int length)
  22. {
  23.   struct pi_skb *nskb;
  24.   unsigned char * msg = m;
  25.  
  26. #ifdef DEBUG
  27.   fprintf(stderr,"-----------------\n");
  28. #endif
  29.  
  30.   /*ps->laddr.pi_port = msg[0];
  31.   ps->raddr.pi_port = msg[1];
  32.   ps->protocol = msg[2]; XXX */
  33.   /*ps->xid = msg[3];*/
  34.   
  35.   if ((!ps->xid) || (ps->xid==0xff)) ps->xid = 0x11; /* some random # */
  36.   ps->xid++;
  37.   ps->xid &= 0xff;
  38.   if ((!ps->xid) || (ps->xid==0xff)) ps->xid = 0x11; /* some random # */
  39.                  
  40.   nskb = (struct pi_skb *)malloc(sizeof(struct pi_skb));
  41.   nskb->source = msg[0];
  42.   nskb->dest = msg[1];
  43.   nskb->type = msg[2];
  44.   nskb->id = ps->xid;
  45.   
  46.   memcpy(&nskb->data[10], msg+4, length-4);
  47.   slp_tx(ps, nskb, length-4);
  48.   
  49.   pi_serial_flush(ps);
  50.  
  51.   return 0;
  52. }
  53.  
  54. int syspkt_rx(struct pi_socket *ps, void *b, int len)
  55. {
  56.   struct pi_skb *skb;
  57.   unsigned char * buf = b;
  58.   int rlen =0;
  59.   
  60.   if (!ps->rxq)
  61.     ps->serial_read(ps, 1);
  62.   
  63.   if (!ps->rxq)
  64.     return 0;
  65.  
  66.   skb = ps->rxq;
  67.   ps->rxq = skb->next;
  68.   
  69.   rlen = skb->len-12;
  70.   
  71.   buf[0] = skb->source;
  72.   buf[1] = skb->dest;
  73.   buf[2] = skb->type;
  74.   buf[3] = skb->id;
  75.  
  76.   memcpy(buf+4, &skb->data[10], rlen);
  77.   
  78.   free(skb);
  79.   return rlen+4;
  80.  
  81. }
  82.  
  83.  
  84. int sys_UnpackState(void * buffer, struct Pilot_state * s)
  85. {
  86.   int i;
  87.   unsigned char * data = buffer;
  88.   
  89.   s->reset = get_short(data);
  90.   s->exception = get_short(data+2);
  91.   memcpy(s->func_name, data+152, 32);
  92.   memcpy(s->instructions, data+78, 30);
  93.   s->func_name[32-1] = 0;
  94.   s->func_start = get_long(data+144);
  95.   s->func_end = get_long(data+148);
  96.   sys_UnpackRegisters(data+4, &s->regs);
  97.  
  98.   for (i=0;i<6;i++) {
  99.     s->breakpoint[i].address = get_long(data+108+i*6);
  100.     s->breakpoint[i].enabled = get_byte(data+112+i*6);
  101.   }
  102.   
  103.   s->trap_rev = get_short(data+184);
  104.   
  105.   return 0;
  106. }
  107.  
  108. int sys_UnpackRegisters(void * data, struct Pilot_registers * r)
  109. {
  110.   unsigned char * buffer = data;
  111.   r->D[0] = get_long(buffer+0);
  112.   r->D[1] = get_long(buffer+4);
  113.   r->D[2] = get_long(buffer+8);
  114.   r->D[3] = get_long(buffer+12);
  115.   r->D[4] = get_long(buffer+16);
  116.   r->D[5] = get_long(buffer+20);
  117.   r->D[6] = get_long(buffer+24);
  118.   r->D[7] = get_long(buffer+28);
  119.   r->A[0] = get_long(buffer+32);
  120.   r->A[1] = get_long(buffer+36);
  121.   r->A[2] = get_long(buffer+40);
  122.   r->A[3] = get_long(buffer+44);
  123.   r->A[4] = get_long(buffer+48);
  124.   r->A[5] = get_long(buffer+52);
  125.   r->A[6] = get_long(buffer+56);
  126.   r->USP = get_long(buffer+60);
  127.   r->SSP = get_long(buffer+64);
  128.   r->PC = get_long(buffer+68);
  129.   r->SR = get_short(buffer+72);
  130.   
  131.   return 0;
  132. }
  133.  
  134. int sys_PackRegisters(void * data, struct Pilot_registers * r)
  135. {
  136.   unsigned char * buffer = data;
  137.   int i;
  138.   for (i=0;i<8;i++)
  139.     set_long(buffer+i*4, r->D[i]);
  140.   for (i=0;i<7;i++)
  141.     set_long(buffer+32+i*4, r->A[i]);
  142.   set_long(buffer+60, r->USP);
  143.   set_long(buffer+64, r->SSP);
  144.   set_long(buffer+68, r->PC);
  145.   set_short(buffer+72, r->SR);
  146.   
  147.   return 0;
  148. }
  149.  
  150. int sys_Continue(int sd, struct Pilot_registers * r, struct Pilot_watch * w)
  151. {
  152.   char buf[94];
  153.   
  154.   buf[0] = 0;
  155.   buf[1] = 0;
  156.   buf[2] = 0;
  157.   buf[3] = 0;
  158.   
  159.   buf[4] = 0x07;
  160.   buf[5] = 0; /*gapfil*/
  161.   
  162.   if (!r)
  163.     return pi_write(sd,buf,6);
  164.   
  165.   sys_PackRegisters(buf+6, r);
  166.   set_byte(buf+80, (w != 0) ? 1 : 0);
  167.   set_byte(buf+81, 0);
  168.   set_long(buf+82, w ? w->address : 0);
  169.   set_long(buf+86, w ? w->length : 0);
  170.   set_long(buf+90, w ? w->checksum : 0);
  171.   
  172.   return pi_write(sd, buf, 94);
  173. }
  174.  
  175. int sys_Step(int sd)
  176. {
  177.   char buf[94];
  178.   
  179.   buf[0] = 0;
  180.   buf[1] = 0;
  181.   buf[2] = 0;
  182.   buf[3] = 0;
  183.   
  184.   buf[4] = 0x03;
  185.   buf[5] = 0; /*gapfil*/
  186.   
  187.   return pi_write(sd,buf,6);
  188. }
  189.  
  190. int sys_SetBreakpoints(int sd, struct Pilot_breakpoint * b)
  191. {
  192.   char buf[94];
  193.   int i;
  194.   
  195.   buf[0] = 0;
  196.   buf[1] = 0;
  197.   buf[2] = 0;
  198.   buf[3] = 0;
  199.   
  200.   buf[4] = 0x0c;
  201.   buf[5] = 0; /*gapfil*/
  202.   
  203.   for (i=0;i<6;i++) {
  204.     set_long(buf+6+i*6, b[i].address);
  205.     set_byte(buf+10+i*6, b[i].enabled);
  206.     set_byte(buf+11+i*6, 0);
  207.   }
  208.   
  209.   pi_write(sd, buf, 42);
  210.   
  211.   i = pi_read(sd, buf, 6);
  212.  
  213.   if ((i<=0) || (buf[4] != (char)0x8c))
  214.     return 0;
  215.   else
  216.     return 1;
  217. }
  218.  
  219. int sys_SetTrapBreaks(int sd, int * traps)
  220. {
  221.   char buf[94];
  222.   int i;
  223.   
  224.   buf[0] = 0;
  225.   buf[1] = 0;
  226.   buf[2] = 0;
  227.   buf[3] = 0;
  228.   
  229.   buf[4] = 0x11;
  230.   buf[5] = 0; /*gapfil*/
  231.   
  232.   for (i=0;i<5;i++) {
  233.     set_short(buf+6+i*2, traps[i]);
  234.   }
  235.   
  236.   pi_write(sd, buf, 16);
  237.   
  238.   i = pi_read(sd, buf, 6);
  239.  
  240.   if ((i<=0) || (buf[4] != (char)0x91))
  241.     return 0;
  242.   else
  243.     return 1;
  244. }
  245.  
  246. int sys_GetTrapBreaks(int sd, int * traps)
  247. {
  248.   char buf[94];
  249.   int i;
  250.   
  251.   buf[0] = 0;
  252.   buf[1] = 0;
  253.   buf[2] = 0;
  254.   buf[3] = 0;
  255.   
  256.   buf[4] = 0x10;
  257.   buf[5] = 0; /*gapfil*/
  258.   
  259.   pi_write(sd, buf, 6);
  260.   
  261.   i = pi_read(sd, buf, 16);
  262.  
  263.   if ((i<16) || (buf[4] != (char)0x90))
  264.     return 0;
  265.  
  266.   for (i=0;i<5;i++) {
  267.     traps[i] = get_short(buf+6+i*2);
  268.   }
  269.  
  270.   return 1;
  271. }
  272.  
  273. int sys_ToggleDbgBreaks(int sd)
  274. {
  275.   char buf[94];
  276.   int i;
  277.   
  278.   buf[0] = 0;
  279.   buf[1] = 0;
  280.   buf[2] = 0;
  281.   buf[3] = 0;
  282.   
  283.   buf[4] = 0x0d;
  284.   buf[5] = 0; /*gapfil*/
  285.   
  286.   pi_write(sd, buf, 6);
  287.   
  288.   i = pi_read(sd, buf, 7);
  289.  
  290.   if ((i<7) || (buf[4] != (char)0x8d))
  291.     return 0;
  292.  
  293.   return get_byte(buf+6);
  294. }
  295.  
  296. int sys_QueryState(int sd)
  297. {
  298.   char buf[6];
  299.   
  300.   buf[0] = 0;
  301.   buf[1] = 0;
  302.   buf[2] = 0;
  303.   buf[3] = 0;
  304.   
  305.   buf[4] = 0;
  306.   buf[5] = 0; /*gapfil*/
  307.   
  308.   return pi_write(sd, buf, 6);
  309. }
  310.  
  311. int sys_ReadMemory(int sd, unsigned long addr, unsigned long len, void * dest)
  312. {
  313.   int result;
  314.   unsigned char buf[0xffff];
  315.   unsigned long todo,done;
  316.   
  317.   
  318.   done = 0;
  319.   do {
  320.     todo = len;
  321.     if (todo > 256)
  322.       todo = 256;
  323.  
  324.     buf[0] = 0;
  325.     buf[1] = 0;
  326.     buf[2] = 0;
  327.     buf[3] = 0;
  328.   
  329.     buf[4] = 0x01;
  330.     buf[5] = 0; /*gapfil*/
  331.   
  332.     set_long(buf+6, addr+done);
  333.     set_short(buf+10, todo);
  334.   
  335.     pi_write(sd, buf, 12);
  336.   
  337.     result = pi_read(sd, buf, todo+6);
  338.   
  339.     if (result<0)
  340.       return done;
  341.   
  342.     if ((buf[4] == 0x81) && ((unsigned int)result == todo+6)) {
  343.       memcpy(((char *)dest) + done, buf+6, todo);
  344.       done += todo;
  345.     } else {
  346.       return done;
  347.     }
  348.   } while (done < len);
  349.   return done;
  350. }
  351.  
  352. int sys_WriteMemory(int sd, unsigned long addr, unsigned long len, void * src)
  353. {
  354.   int result;
  355.   unsigned char buf[0xffff];
  356.   unsigned long todo, done;
  357.   
  358.   
  359.   done = 0;
  360.   do {
  361.     todo = len;
  362.     if (todo>256)
  363.       todo = 256;
  364.  
  365.     buf[0] = 0;
  366.     buf[1] = 0;
  367.     buf[2] = 0;
  368.     buf[3] = 0;
  369.   
  370.     buf[4] = 0x02;
  371.     buf[5] = 0; /*gapfil*/
  372.  
  373.   
  374.     set_long(buf+6, addr);
  375.     set_short(buf+10, len);
  376.     memcpy(buf+12, ((char *)src)+done, todo);
  377.   
  378.     pi_write(sd, buf, len+12);
  379.   
  380.     result = pi_read(sd, buf, 6);
  381.   
  382.     if (result<0)
  383.       return done;
  384.    
  385.     if ((buf[4] == 0x82) && ((unsigned int)result == todo+6)) {
  386.       ;
  387.     } else {
  388.       return done;
  389.     }
  390.   } while (done < len);
  391.   return done;
  392. } 
  393.  
  394. int sys_Find(int sd, unsigned long startaddr, unsigned long stopaddr, int len, int caseinsensitive,
  395.              void * data, unsigned long * found)
  396. {
  397.   int result;
  398.   unsigned char buf[0xffff];
  399.   
  400.   buf[0] = 0;
  401.   buf[1] = 0;
  402.   buf[2] = 0;
  403.   buf[3] = 0;
  404.   
  405.   buf[4] = 0x11;
  406.   buf[5] = 0; /*gapfil*/
  407.   
  408.   set_long(buf+6, startaddr);
  409.   set_long(buf+10, stopaddr);
  410.   set_short(buf+14, len);
  411.   set_byte(buf+16, caseinsensitive);
  412.   memcpy(buf+17, data, len);
  413.   
  414.   pi_write(sd, buf, len+17);
  415.   
  416.   result = pi_read(sd, buf, 12);
  417.   
  418.   if (result<0)
  419.     return result;
  420.     
  421.   if (found)
  422.     *found = get_long(buf+6);
  423.   
  424.   return get_byte(buf+10);
  425. }
  426.  
  427.  
  428. int sys_RemoteEvent(int sd, int penDown, int x, int y, int keypressed, 
  429.                        int keymod, int keyasc, int keycode)
  430. {
  431.   char buf[20];
  432.   
  433.   buf[0] = 2;
  434.   buf[1] = 2;
  435.   buf[2] = 0;
  436.   buf[3] = 0x11;
  437.   
  438.   buf[4] = 0x0d; /*RemoteEvtCommand*/
  439.   buf[5] = 0; /*gapfil*/
  440.   buf[6] = penDown;
  441.   buf[7] = 0; /*gapfil*/
  442.   buf[8] = x >> 8;
  443.   buf[9] = x & 0xff;
  444.   buf[10] = y >> 8;
  445.   buf[11] = y & 0xff;
  446.   buf[12] = keypressed;
  447.   buf[13] = 0; /*gapfil*/
  448.   buf[14] = keymod >> 8;
  449.   buf[15] = keymod & 0xff;
  450.   buf[16] = keyasc >> 8;
  451.   buf[17] = keyasc & 0xff;
  452.   buf[18] = keycode >> 8;
  453.   buf[19] = keycode & 0xff;
  454.   
  455.   return pi_write(sd, buf, 16+4);
  456. }
  457.  
  458. int sys_RPC(int sd, int socket, int trap, long * D0, long * A0, int params, struct RPC_param * param, int reply)
  459. {
  460.   unsigned char buf[4096];
  461.   int i;
  462.   unsigned char * c;
  463.   
  464.   buf[0] = socket; /* 0 for debug, 1 for console */
  465.   buf[1] = socket;
  466.   buf[2] = 0;
  467.   buf[4] = 0x0a;
  468.   buf[5] = 0;
  469.   
  470.   set_short(buf+6, trap);
  471.   set_long(buf+8, *D0);
  472.   set_long(buf+12, *A0);
  473.   set_short(buf+16, params);
  474.   
  475.   c = buf+18;
  476.   for(i=params-1;i>=0;i--) {
  477.     set_byte(c, param[i].byRef); c++;
  478.     set_byte(c, param[i].size); c++;
  479.     if(param[i].data)
  480.       memcpy(c, param[i].data, param[i].size);
  481.     c += param[i].size;
  482.     if (param[i].size & 1)
  483.       *c++ = 0;
  484.   }
  485.   
  486.   if (socket == 3)
  487.     set_short(buf+4, c-buf - 6);
  488.   
  489.   pi_write(sd, buf, c-buf);
  490.   
  491.   if(reply) {
  492.     int l = pi_read(sd, buf, 4096);
  493.   
  494.     if (l < 0)
  495.       return l;
  496.     if (l < 6)
  497.       return -1;
  498.     if (buf[4] != 0x8a)
  499.       return -2;
  500.     
  501.     *D0 = get_long(buf+8);
  502.     *A0 = get_long(buf+12);
  503.     c = buf+18;
  504.     for(i=params-1;i>=0;i--) {
  505.       if(param[i].byRef && param[i].data)
  506.         memcpy(param[i].data, c+2, param[i].size);
  507.       c += 2 + ((get_byte(c+1) + 1)& ~1);
  508.     }
  509.   }
  510.   return 0;
  511. }
  512.  
  513. /* Deprecated */
  514. int RPC(int sd, int socket, int trap, int reply, ...)
  515. {
  516.   va_list ap;
  517.   struct RPC_param p[20];
  518.   int RPC_arg[20];
  519.   int i=0,j;
  520.   long D0=0,A0=0;
  521.  
  522.   va_start(ap, reply);
  523.   for(;;) {
  524.     int type = va_arg(ap, int);
  525.     if(type == 0)
  526.       break;
  527.     if(type < 0) {
  528.       p[i].byRef = 0;
  529.       p[i].size = -type;
  530.       RPC_arg[i] = va_arg(ap,int);
  531.       p[i].data = &RPC_arg[i];
  532.       p[i].invert = 0;
  533.     } else {
  534.       void * c = va_arg(ap,void*);
  535.       p[i].byRef = 1;
  536.       p[i].size = type;
  537.       p[i].data = c;
  538.       p[i].invert = va_arg(ap,int);
  539.       if(p[i].invert) {
  540.         if(p[i].size == 2) {
  541.           int * s = c;
  542.           *s = htons(*s);
  543.         } else {
  544.           int * l = c;
  545.           *l = htonl(*l);
  546.         }
  547.       }
  548.     }
  549.     i++;
  550.   }
  551.   va_end(ap);
  552.   
  553.   sys_RPCerror = sys_RPC(sd, socket, trap, &D0, &A0, i, p, reply != 2);
  554.   
  555.   for(j=0;j<i;j++) {
  556.       if(p[j].invert) {
  557.         void * c = p[j].data;
  558.         if(p[j].size == 2) {
  559.           int * s = c;
  560.           *s = htons(*s);
  561.         } else {
  562.           int * l = c;
  563.           *l = htonl(*l);
  564.         }
  565.       }
  566.   }
  567.   
  568.   if(reply)
  569.     return A0;
  570.   else
  571.     return D0;
  572. }
  573.  
  574. int PackRPC(struct RPC_params * p, int trap, int reply, ...)
  575. {
  576.   va_list ap;
  577.   int i=0;
  578.   
  579.   p->trap = trap;
  580.   p->reply = reply;
  581.  
  582.   va_start(ap, reply);
  583.   for(;;) {
  584.     int type = (int)va_arg(ap, int);
  585.     if(type == 0)
  586.       break;
  587.     if(type < 0) {
  588.       p->param[i].byRef = 0;
  589.       p->param[i].size = -type;
  590.       p->param[i].arg = (int)va_arg(ap, int);
  591.       p->param[i].data = &p->param[i].arg;
  592.       p->param[i].invert = 0;
  593.     } else {
  594.       void * c = (void*)va_arg(ap,void*);
  595.       p->param[i].byRef = 1;
  596.       p->param[i].size = type;
  597.       p->param[i].data = c;
  598.       p->param[i].invert = (int)va_arg(ap,int);
  599.     }
  600.     i++;
  601.   }
  602.   p->args = i;
  603.   va_end(ap);
  604.   
  605.   return 0;
  606. }
  607.  
  608. void UninvertRPC(struct RPC_params * p)
  609. {
  610.   int j;
  611.   
  612.   for(j=0;j<p->args;j++) {
  613.       if(p->param[j].invert) {
  614.         void * c = p->param[j].data;
  615.         if((p->param[j].invert == 2) && (p->param[j].size == 2)) {
  616.           int * s = c;
  617.           *s = htons(*s)>>8;
  618.         } else if(p->param[j].size == 2) {
  619.           int * s = c;
  620.           *s = htons(*s);
  621.         } else {
  622.           long * l = c;
  623.           *l = htonl(*l);
  624.         }
  625.       }
  626.   }
  627. }
  628.  
  629. void InvertRPC(struct RPC_params * p)
  630. {
  631.   int j;
  632.   
  633.   for(j=0;j<p->args;j++) {
  634.       if(p->param[j].invert) {
  635.         void * c = p->param[j].data;
  636.         if((p->param[j].invert == 2) && (p->param[j].size == 2)) {
  637.           int * s = c;
  638.           *s = ntohs(*s)>>8;
  639.         } else if(p->param[j].size == 2) {
  640.           int * s = c;
  641.           *s = ntohs(*s);
  642.         } else {
  643.           long * l = c;
  644.           *l = ntohl(*l);
  645.         }
  646.       }
  647.   }
  648. }
  649.  
  650.  
  651. unsigned long DoRPC(int sd, int socket, struct RPC_params * p, int * error)
  652. {
  653.   int err;
  654.   long D0=0,A0=0;
  655.   
  656.   InvertRPC(p);
  657.  
  658.   err = sys_RPC(sd, socket, p->trap, &D0, &A0, p->args, &p->param[0], p->reply);
  659.   
  660.   UninvertRPC(p);
  661.   
  662.   if (error)
  663.     *error = err;
  664.   
  665.   if(p->reply==RPC_PtrReply)
  666.     return A0;
  667.   else if (p->reply==RPC_IntReply)
  668.     return D0;
  669.   else
  670.     return err;
  671. }
  672.  
  673. int RPC_Int_Void(int sd, int trap) {
  674.   return RPC(sd, 1, trap, 0, RPC_End);
  675. }
  676.  
  677. int RPC_Ptr_Void(int sd, int trap) {
  678.   return RPC(sd, 1, trap, 1, RPC_End);
  679. }
  680.  
  681. /* Untested complex RPC example
  682. int RPC_MemCardInfo(int sd, int cardno, char * cardname, char * manufname,
  683.                     int * version, long * date, long * romsize, long * ramsize,
  684.                     long * freeram) {
  685.   return RPC(sd, 1, 0xA004, 0, RPC_Short(cardno), RPC_Ptr(cardname, 32), 
  686.                                RPC_Ptr(manufname, 32), RPC_ShortPtr(version),
  687.                                RPC_LongPtr(date), RPC_LongPtr(romsize),
  688.                                RPC_LongPtr(ramsize), RPC_LongPtr(freeram));
  689. }                    
  690. */
  691.